﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace PowerControl
{
    public class MobileBehaviorWander : MobileBehavior
    {
        private Random rng;
        public MobileBehaviorWander()
        {
            rng = new Random(this.GetHashCode());
        }

        public void Step(double DeltaT, MobileDevice m)
        {
            double steeringAngle = (-Math.PI/16.0) + rng.NextDouble() * Math.PI / 8.0;
            double centerX = Math.Cos(m.Position.Yaw) + m.Position.X;
            double centerY = Math.Sin(m.Position.Yaw) + m.Position.Y;

            //Compute new angle
            double newAngle = Math.Max(m.AngularSpeed * DeltaT, m.Position.Yaw - steeringAngle);
            m.Position.Yaw = newAngle;

            //Move forward toward the new direction
            m.Position.X += Math.Cos( newAngle ) * DeltaT * m.Speed;
            m.Position.Y += Math.Sin( newAngle ) * DeltaT * m.Speed;
        }
    }
}
